#include <iostream>
#include <Eigen/Geometry>
#include <sophus/so3.hpp>

using namespace std;
using namespace Eigen;
using namespace Sophus;

int main()
{
    AngleAxisd r(M_PI_2, Vector3d::UnitZ());

    Quaterniond q(r);

    SO3d so3_r(r.matrix());
    SO3d so3_q(q);

    cout << "SO3 from matrix: \n"
         << so3_r.matrix() << endl;
    cout << "SO3 from quaternion: \n"
         << so3_q.matrix() << endl;

    Vector3d so3 = so3_r.log(); // Lie Algebra
    cout << "so3 = " << so3.transpose() << endl;
    cout << "so3^ = \n"
         << SO3d::hat(so3) << endl;

    Vector3d so3_delta(1e-4, 0, 0);
    SO3d so3_r_update = SO3d::exp(so3_delta) * so3_r;
    cout << "SO3 update = \n"
         << so3_r_update.matrix() << endl;

    return 0;
}